#coding=utf - 8
import rospy
import math
import time
import USART
import os
import sys
import fcntl
import threading

path = os.path.abspath(".")
sys.path.insert(0,path + "/src/car_pkg/scripts")
#sys.path.insert(0,"/home/sunrise/workspace/yolov5-master/")

sender = USART.SerialSender("/dev/ttyS4", 115200)
print("/dev/ttyS4;115200")
received_array=[1,1,1,1,1,1]
flag_fire1 = 1
flag = 0
biaozhi = 0
center_zuobiao = [0,0]
distance = 0


def car_forward():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x01
  received_array[5]=0xAA
  sender.air_speed(received_array)


def car_left_turn():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x03
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
def car_right_turn():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x01
  received_array[4]=0x04
  received_array[5]=0xAA
  sender.air_speed(received_array)

def car_stop():
  received_array[0]=0xFF
  received_array[1]=0x01
  received_array[2]=0x00
  received_array[3]=0x00
  received_array[4]=0x05
  received_array[5]=0xAA
  sender.air_speed(received_array)
  
def car_for_distance(distance):
  received_array[2] = int(distance)
  #距离小数值
  received_array[3] = int(((distance % 1) * 10)+2)  
  received_array[0]=0xFF
  received_array[1]=0x02
  received_array[4]=0x01
  received_array[5]=0xAA
  sender.air_speed(received_array)
  distime = distance / 0.1
  time.sleep(distime + 10)


def fire_turn():
  global flag_fire1 
  print('fire')
  flag_fire1 = 2
  received_array[0]=0xFF
  received_array[1]=0x04
  received_array[2]=0x00
  received_array[3]=0x00
  received_array[4]=0x08
  received_array[5]=0xAA
  sender.air_speed(received_array)
    
  
  
def fire_stop():
  global flag_fire1 
  if flag_fire1 == 3:
    flag_fire1 = 1
    received_array[0]=0xFF
    received_array[1]=0x04
    received_array[2]=0x00
    received_array[3]=0x00
    received_array[4]=0x07
    received_array[5]=0xAA
    sender.air_speed(received_array)


def fire_check():
    global center_zuobiao
    global biaozhi
    global distance
    global flag
    count = 0
    while True:
        try:
           file_path = "/home/orangepi/car_web/src/car_pkg/scripts/zuobiao.txt"
           file = open(file_path, "r+")
           # 尝试获取文件锁，如果文件被其他进程独占写，会阻塞
           fcntl.flock(file.fileno(), fcntl.LOCK_SH | fcntl.LOCK_NB)
           content = file.readline()
           content = content.strip()
           elements = content.split(",")
           if len(content) == 0:
              continue
           else:
               if elements[2] not in ["1", "0"] or len(elements)>5:
                 file.seek(0)
                 file.write("000,000,0,0,0.00")
               elif elements[2] == "1":
                 center_zuobiao = [int(elements[0]), int(elements[1])]
                 if count == 1 and float(elements[4])>0 and float(elements[4]!=distance):
                    distance = float(elements[4])
                    flag =0
                    #print(distance)
                 index = content.find("1")
                 file.seek(index)
                 file.write("0")
                 if center_zuobiao[0] >= 320 and center_zuobiao[0] <= 400:
                    file.seek(index + 1)
                    file.write(",2")
                    count = 1

                 elif (center_zuobiao[0] < 320 or center_zuobiao[0] > 400) and center_zuobiao[0]!=0:
                    file.seek(index + 1)
                    file.write(",0")
                    count = 0
                                  
           # 释放文件锁
           fcntl.flock(file.fileno(), fcntl.LOCK_UN)
           file.close()
           #print(center_zuobiao)
        except Exception as e:
           print(f"An error occurred: {e}")   

def motion_car():
  global center_zuobiao
  global biaozhi
  global distance
  global flag_fire1 
  global flag
  target_distance = 0.9
  num = 0
  while True:
    try:  
      if (center_zuobiao[0] >= 320 and center_zuobiao[0] <= 400) or (distance <= target_distance and distance >0):
        if num == 0 :#判断是否为第一次进入测距模式
           car_stop()
           num = 1
        #如果坐标对准了，就停止小车
        #判断距离值是否在合理喷射灭火器的范围内
        if distance <= target_distance and distance >0:
          print(distance)
          #在合理范围之内就要停止运动，并进行灭火启动
          car_stop()
          time.sleep(2)
          if flag_fire1 == 1 :
             fire_turn()
             time.sleep(2)
          
        elif distance > target_distance and flag ==0:
          print('gogogo')
          #不在合理范围之内要进行前进
          #car_forward()
          print(distance)
          car_for_distance(distance-target_distance)
          flag =1
      
      elif (center_zuobiao[0] < 320 or center_zuobiao[0] > 400) and center_zuobiao[0]!=0 and flag_fire1!=2:   
        #如果坐标值没有对准就要进行判断左转或者右转
        if center_zuobiao[0]>400:
          print('right')
          car_right_turn()
        if center_zuobiao[0]<320:
          print('left')
          car_left_turn()
        num = 0
      
    except Exception as e:
      print(f"An error occured in motion__car thread: {e}")


def file_init():
  file_path = "/home/orangepi/car_web/src/car_pkg/scripts/zuobiao.txt"
  file = open(file_path, "w+")
  # 尝试获取文件锁，如果文件被其他进程独占写，会阻塞
  fcntl.flock(file.fileno(), fcntl.LOCK_SH | fcntl.LOCK_NB)
  file.seek(0)
  file.write("0,0,0,0,0.00")
  # 释放文件锁
  fcntl.flock(file.fileno(), fcntl.LOCK_UN)
  file.close()


def thread_main():
  #初始化文件读取
  file_init()
  #多线程跑火焰检测以及小车的移动
  fire_check_thread=threading.Thread(target=fire_check)
  motion_car_thread=threading.Thread(target=motion_car)
  fire_check_thread.start()
  motion_car_thread.start()
  fire_check_thread.join()
  motion_car_thread.join()


if __name__ == "__main__":
  thread_main()
    
    





    


    
    
